#include "Boid.h"

Boid::Boid(vector<Boid*> *hostBoidList, lqDB* lqDatabase, int id)
{
    boids = hostBoidList;
    hostLqDB = lqDatabase;
    idNr_ = id;

	// physical properties
	mass_ = 1.0;
    radius_ = 1.0;
    boid_color.r = 1.0;
    boid_color.g = 1.0;
    boid_color.b = 1.0;
    trail_length = 10;

	// steering properties
    separationWeight = 0.0;
	alignmentWeight = 0.0;
	cohesionWeight = 0.0;
    separationRadius = 10.0;
    neighborhoodRadius = 20.0;
    neighborhoodAngle = -0.5;
	maxForce_ = 0.2;
    maxSpeed_ = 0.8;
    forward_.set(1.0, 0.0); // x basis vector
    side_.set   (0.0, 1.0); // y basis vector
    //up_.set     (0.0, 1.0, 0.0);


    // set random position and velocity
    position_.set( ofRandomWidth(), ofRandomHeight());
	velocity_.set( ofRandom(-maxSpeed_, maxSpeed_),ofRandom(-maxSpeed_, maxSpeed_) );
	velocity_.limit(maxSpeed_);

    speed_ = velocity_.length();
    if (speed_ > 0)
        forward_ = velocity_ / speed_; // set forward vector
    side_ = forward_.getPerpendicular(); // derive new unit side basis vector from forward and up

    // create a pointer to this object in proximity database
    lqInitClientProxy(&lqClient, this);

}


Boid::~Boid(void)
{
    lqRemoveFromBin(&lqClient);
}

void Boid::flock()
{
    separationAngle = neighborhoodAngle;
    alignmentRadius = neighborhoodRadius;
    alignmentAngle  = neighborhoodAngle;
    cohesionRadius  = neighborhoodRadius;
    cohesionAngle   = neighborhoodAngle;

    //if ( (rand()%6) == 1) /// FIX make this evenly spaced
    {
            //steering_direction = steerForWander(ofGetLastFrameTime());
            //steering_direction = steerForPursuit(*quarry);
            steering_direction = steerToFlock();
    }
}


// ----------------------------------------------------------------------------
// apply a given steering force to our momentum,
// adjusting our orientation to maintain velocity-alignment.

void Boid::applySteeringForce (Vec2 force, const float elapsedTime)
{
    // disallow backward-facing steering at low speed.
    const float maxAdjustedSpeed = 0.2f * maxSpeed_;

    if ( (speed_ < maxAdjustedSpeed) && (force.squareLength() > 0.0) )
    {
        const float range = speed_ / maxAdjustedSpeed;
        // const float cosine = interpolate (pow (range, 6), 1.0f, -1.0f);
        // const float cosine = interpolate (pow (range, 10), 1.0f, -1.0f);
        // const float cosine = interpolate (pow (range, 20), 1.0f, -1.0f);
        // const float cosine = interpolate (pow (range, 100), 1.0f, -1.0f);
        // const float cosine = interpolate (pow (range, 50), 1.0f, -1.0f);
        //const float cosine = interpolate (pow (range, 20), 1.0f, -1.0f);
        const float cosine = 1.0 - 2.0 * pow(range, 20);
        //return limitMaxDeviationAngle (force, cosine, forward());

    }

    // enforce limit on magnitude of steering force
    const Vec2 clippedForce = force.limited(maxForce_);

    // compute acceleration and velocity
    Vec2 newAcceleration = clippedForce / mass_;
    Vec2 newVelocity = forward_ * speed_;

    // damp out abrupt changes and oscillations in steering acceleration
    // (rate is proportional to time step, then clipped into useful range)
    if (elapsedTime > 0.0)
    {
        const float smoothRate = ofClamp(9.0 * elapsedTime, 0.15, 0.4);
        smoothedAcceleration += (newAcceleration - smoothedAcceleration) *
                                 smoothRate;
    }

    // Euler integrate (per frame) acceleration into velocity
    newVelocity += smoothedAcceleration;/// * elapsedTime;

    // enforce speed limit
    newVelocity.limit(maxSpeed_);

    // update Speed
    speed_ = newVelocity.length();

    // Euler integrate (per frame) velocity into position
    position_ += newVelocity;/// * elapsedTime;

    // adjust orthonormal basis vectors to be aligned with new velocity
    if (speed_ > 0.0)
    {
        forward_ = newVelocity / speed_;

        // derive new unit side basis vector from forward and up
        side_ = forward_.getPerpendicular();

        // derive new Up basis vector from new Side and new Forward
        ///up_ = side_.getPerpendicular(forward_);
    }
}


Vec2 Boid::steerToFlock (void)
{
    vector<Boid*> neighbors;// = *boids; // temporary    // find all flockmates within maxRadius using proximity database
    findNeighbors (neighbors);

    // determine each of the three component behaviors of flocking
    const Vec2 separation = steerForSeparation (separationRadius, separationAngle, neighbors);
    const Vec2 alignment = steerForAlignment (alignmentRadius, alignmentAngle, neighbors);
    const Vec2 cohesion = steerForCohesion (cohesionRadius, cohesionAngle, neighbors);

    // apply weights to components
    const Vec2 separationW = separation * separationWeight;
    const Vec2 alignmentW = alignment * alignmentWeight;
    const Vec2 cohesionW = cohesion * cohesionWeight;

    return separationW + alignmentW + cohesionW;
}


void Boid::update (void)
{
    applySteeringForce(steering_direction, ofGetLastFrameTime());

    trail.push_back(position_);
    while (trail.size() > trail_length)
        trail.pop_front();

    // wrap around screen edges
    borders();

    // notify proximity database that our position has changed
    lqUpdateForNewLocation (hostLqDB, &lqClient, position_.x, position_.y, position_.z);
}


void Boid::draw()
{
	/// Actual position
    ofFill();
	ofSetColor((int)(boid_color.r*0xff), (int)(boid_color.g*0xff), (int)(boid_color.b*0xff), 0xff);
	ofCircle(position_.x, position_.y, radius_);

    /// Arrow
	if (idNr_ == 2)
	{
	    const float arrowLength = 50.0;
	    const Vec2 forwardVect = ofxVec2f(position_.x + forward_.x * arrowLength,
                                          position_.y + forward_.y * arrowLength);
        const Vec2 sideVect = ofxVec2f(position_.x + side_.x * arrowLength,
                                       position_.y + side_.y * arrowLength);
	    ofSetColor(0xff0000);
	    ofLine(position_.x, position_.y, forwardVect.x, forwardVect.y);
	    ofSetColor(0x00ff00);
	    ofLine(position_.x, position_.y, sideVect.x, sideVect.y);
        //cout << "forward x: " << forward_.x << " forward y: " << forward_.y << " forward z: " << forward_.z << endl;
        //cout << "side x: " << side_.x << " side y: " << side_.y << " side z: " << side_.z << endl;
        //cout << "up x: " << up_.x << " up y: " << up_.y << " up z: " << up_.z << "\n\n";
	}

    /// Trails
    if (trail.size() > 0)
    {
        ofEnableAlphaBlending();

        float blend_steps = (float)0xff / (float)trail.size(); // use float if trail is longer than 255
        float alpha = 0.0;
        ofSetLineWidth(radius_ * 2.0);
        for (size_t i=0; i<(trail.size()-1); i++)
        {
            ofSetColor((int)(boid_color.r*0xff), (int)(boid_color.g*0xff), (int)(boid_color.b*0xff), (int)alpha);

            // do not draw a trail if the boids wraps around the screen
            float trailLengthX = abs(trail[i].x - trail[i+1].x);
            float trailLengthY = abs(trail[i].y - trail[i+1].y);
            if ( (trailLengthX < 100 ) && (trailLengthY < 100 )  )
                ofLine(trail[i].x, trail[i].y,trail[i+1].x, trail[i+1].y);

            alpha += blend_steps;
        }
        ofDisableAlphaBlending();
    }
}


// ----------------------------------------------------------------------------
// used by boid behaviors: is a given vehicle within this boid's neighborhood?

bool Boid::inBoidNeighborhood (const Boid& other, const float minDistance,
                               const float maxDistance, const float cosMaxAngle)
{
    if (&other == this)
    {
        return false;
    }
    else
    {
        const Vec2 offset = other.position() - position();
        const float distanceSquared = offset.lengthSquared ();

        // definitely in neighborhood if inside minDistance sphere
        if (distanceSquared < (minDistance * minDistance))
        {
            return true;
        }
        else
        {
            // definitely not in neighborhood if outside maxDistance sphere
            if (distanceSquared > (maxDistance * maxDistance))
            {
                return false;
            }
            else
            {
                // otherwise, test angular offset from forward axis
                const Vec2 unitOffset = offset / sqrt (distanceSquared);
                const float forwardness = forward().dot (unitOffset);
                return forwardness > cosMaxAngle;
            }
        }
    }
}


// ----------------------------------------------------------------------------
// Seek behavior

Vec2 Boid::steerForSeek (const Vec2& target)
{
    const Vec2 desiredVelocity = target - position_;
    return desiredVelocity - velocity_;
}


// ----------------------------------------------------------------------------
// Flee behavior

Vec2 Boid::steerForFlee (const Vec2& target)
{
    const Vec2 desiredVelocity = position_ - target;
    return desiredVelocity - velocity_;
}


// ----------------------------------------------------------------------------
// pursuit of another vehicle (& version with ceiling on prediction time)

Vec2 Boid::steerForPursuit (const Boid& quarry)
{
    return steerForPursuit (quarry, HUGE_VALF);
}

Vec2 Boid::steerForPursuit (const Boid& quarry, const float maxPredictionTime)
{
    // offset from this to quarry, that distance, unit vector toward quarry
    const Vec2 offset = quarry.position() - position_;
    const float distance = offset.length();
    const Vec2 unitOffset = offset / distance;

    // how parallel are the paths of "this" and the quarry
    // (1 means parallel, 0 is pependicular, -1 is anti-parallel)
    const float parallelness = forward().dot (quarry.forward());

    // how "forward" is the direction to the quarry
    // (1 means dead ahead, 0 is directly to the side, -1 is straight back)
    const float forwardness = forward().dot (unitOffset);

    const float directTravelTime = distance / speed_;
    ///const int f = intervalComparison (forwardness,  -0.707f, 0.707f);
    const int f = (forwardness < -M_SQRT1_2) ? -1 : ( (forwardness > M_SQRT1_2) ? 1 : 0); // -1 if lower, 0 in between, 1 if over
    ///const int p = intervalComparison (parallelness, -0.707f, 0.707f);
    const int p = (parallelness < -M_SQRT1_2) ? -1 : ( (parallelness > M_SQRT1_2) ? 1 : 0);

    float timeFactor = 0; // to be filled in below
    //ofColor color;           // to be filled in below (xxx just for debugging)

    // Break the pursuit into nine cases, the cross product of the
    // quarry being [ahead, aside, or behind] us and heading
    // [parallel, perpendicular, or anti-parallel] to us.
    switch (f)
    {
    case +1:
        switch (p)
        {
        case +1:          // ahead, parallel
            timeFactor = 4.0f;
            //color.r = 139; color.g = 69; color.b = 19; // brown
            break;
        case 0:           // ahead, perpendicular
            timeFactor = 1.8f;
            //color.r = 127; color.g = 127; color.b = 127; // grey50
            break;
        case -1:          // ahead, anti-parallel
            timeFactor = 0.85f;
            //color.r = 255; color.g = 255; color.b = 255; // white
            break;
        }
        break;
    case 0:
        switch (p)
        {
        case +1:          // aside, parallel
            timeFactor = 1.0f;
            //color.r = 255; color.g = 0; color.b = 0; // red
            break;
        case 0:           // aside, perpendicular
            timeFactor = 0.8f;
            //color.r = 255; color.g = 255; color.b = 0; // yellow
            break;
        case -1:          // aside, anti-parallel
            timeFactor = 4.0f;
            //color.r = 0; color.g = 255; color.b = 0; // green
            break;
        }
        break;
    case -1:
        switch (p)
        {
        case +1:          // behind, parallel
            timeFactor = 0.5f;
            //color.r = 0; color.g = 255; color.b = 255; // cyan
            break;
        case 0:           // behind, perpendicular
            timeFactor = 2.0f;
            //color.r = 0; color.g = 0; color.b = 255; // blue
            break;
        case -1:          // behind, anti-parallel
            timeFactor = 2.0f;
            //color.r = 255; color.g = 0; color.b = 255; // magenta
            break;
        }
        break;
    }

    // estimated time until intercept of quarry
    const float et = directTravelTime * timeFactor;

    // xxx experiment, if kept, this limit should be an argument
    const float etl = (et > maxPredictionTime) ? maxPredictionTime : et;

    // estimated position of quarry at intercept
    const Vec2 target = quarry.position() + (quarry.velocity() * etl);

    //boid_color = color;

    return steerForSeek (target);
}


// ----------------------------------------------------------------------------
// evasion of another vehicle

Vec2 Boid::steerForEvasion (const Boid& menace, const float maxPredictionTime)
{
    // offset from this to menace, that distance, unit vector toward menace
    const Vec2 offset = menace.position() - position_;
    const float distance = offset.length ();

    const float roughTime = distance / menace.speed();
    const float predictionTime = ((roughTime > maxPredictionTime) ?
                                  maxPredictionTime :
                                  roughTime);

    const Vec2 target = menace.position() + (menace.velocity() * predictionTime);

    return steerForFlee (target);
}


// ----------------------------------------------------------------------------
/// Offset pursuit


// ----------------------------------------------------------------------------
/// Obstacle avoidance


// ----------------------------------------------------------------------------
// Wander behavior

Vec2 Boid::steerForWander (float dt)
{
    static float wanderSide = 0.0;
    static float wanderUp = 0.0;

    // random walk WanderSide and WanderUp between -1 and +1
    const float speed = 12 * dt; // maybe this (12) should be an argument?
    const float sideWalk = wanderSide + ofRandom(-speed, speed);
    const float upWalk = wanderUp + ofRandom(-speed, speed);

    wanderSide = ofClamp(sideWalk, -1.0, 1.0);
    wanderUp = ofClamp(upWalk, -1.0, 1.0);

    // return a pure lateral steering vector: (+/-Side) + (+/-Up)
    return (side_ * wanderSide);// + (up_ * wanderUp);
}


// ----------------------------------------------------------------------------
// Separation behavior: steer away from neighbors

Vec2 Boid::steerForSeparation (const float maxDistance, const float cosMaxAngle, const vector<Boid*>& flock)
{
    // steering accumulator, initially zero
    Vec2 steering;

    // for each of the other vehicles...
    for (vector<Boid*>::const_iterator other = flock.begin(); other != flock.end(); other++)
    {
        if (inBoidNeighborhood (**other, radius_*3, maxDistance, cosMaxAngle))
        {
            // add in steering contribution
            // (opposite of the offset direction, divided once by distance
            // to normalize, divided another time to get 1/d falloff)
            const Vec2 offset = position_ - (**other).position();
            steering += offset / offset.lengthSquared();
        }
    }

    // divide by neighbors, then normalize to pure direction
    // XXX no need to divide by neighbors if we normalize!!
#ifdef PROCESSING_STYLE
    return steering;
#else
    return steering.normalized();
#endif
}


// ----------------------------------------------------------------------------
// Alignment behavior: steer to head in same direction as neighbors

Vec2 Boid::steerForAlignment (const float maxDistance, const float cosMaxAngle, const vector<Boid*>& flock)
{
    // steering accumulator and count of neighbors, both initially zero
    Vec2 steering;
    int neighbors = 0;

    // for each of the other vehicles...
    for (vector<Boid*>::const_iterator other = flock.begin(); other != flock.end(); other++)
    {
        if (inBoidNeighborhood (**other, radius_*3, maxDistance, cosMaxAngle))
        {
            // accumulate sum of neighbor's heading
#ifdef PROCESSING_STYLE
            steering += (**other).velocity();
#else
            steering += (**other).forward();
#endif

            // count neighbors
            neighbors++;
        }
    }

    // divide by neighbors, subtract off current heading to get error-
    // correcting direction, then normalize to pure direction
    if (neighbors > 0)
#ifdef PROCESSING_STYLE
        steering = ((steering / (float)neighbors) - forward_ * speed_);
#else
        steering = ((steering / (float)neighbors) - forward_).normalize();
#endif

    return steering;
}


// ----------------------------------------------------------------------------
// Cohesion behavior: to move toward center of neighbors

Vec2 Boid::steerForCohesion (const float maxDistance, const float cosMaxAngle, const vector<Boid*>& flock)
{
    // steering accumulator and count of neighbors, both initially zero
    Vec2 steering;
    int neighbors = 0;

    // for each of the other vehicles...
    for (vector<Boid*>::const_iterator other = flock.begin(); other != flock.end(); other++)
    {
        if (inBoidNeighborhood (**other, radius_*3, maxDistance, cosMaxAngle))
        {
            // accumulate sum of neighbor's positions
            steering += (**other).position();

            // count neighbors
            neighbors++;
        }
    }

    // divide by neighbors, subtract off current position to get error-
    // correcting direction, then normalize to pure direction
    if (neighbors > 0)
        steering = (steering / (float)neighbors) - position_;

#ifdef PROCESSING_STYLE
    return steering;
#else
    return steering.normalized();
#endif
}


void Boid::borders() {  // Wrap our Boid's position around the screen
    if (position_.x > ofGetWidth())
        position_.x -= ofGetWidth();
    if (position_.x < 0)
        position_.x += ofGetWidth();
    if (position_.y > ofGetHeight())
        position_.y -= ofGetHeight();
    if (position_.y < 0)
        position_.y += ofGetHeight();
}


void Boid::findNeighbors(vector<Boid*>& results)
{
    lqMapOverAllObjectsInLocality (hostLqDB, position_.x, position_.y, position_.z,
                                   neighborhoodRadius, perNeighborCallBackFunction, (void*)&results);
}


// Function to get boid distance wrapped around the screen
Vec2 Boid::check_boid_pos(Boid *nearby_boid, float wrap_radius)
{
    Vec2 wrapped_pos = nearby_boid->position();
    if (position().x < wrap_radius) { // close to left edge
        wrapped_pos.x -= ofGetWidth();
    }
    if ( (ofGetWidth() - position().x) < wrap_radius ) { // close to right edge
        wrapped_pos.x += ofGetWidth();
    }
    if (position().y < wrap_radius) { // close to top
        wrapped_pos.y -= ofGetHeight();
    }
    if ( (ofGetHeight() - position().y) < wrap_radius ) { // close to bottom
        wrapped_pos.y += ofGetHeight();
    }
    return wrapped_pos;
}

// Take a const-reference to the right-hand side of the assignment.
// Return a non-const reference to the left-hand side.
Boid& Boid::operator=(const Boid &rhs) {
    if (this == &rhs)      // Same object?
        return *this;      // Yes, so skip assignment, and just return *this.

    separationWeight = rhs.separationWeight;
    alignmentWeight = rhs.alignmentWeight;
    cohesionWeight = rhs.cohesionWeight;
    separationRadius = rhs.separationRadius;
    neighborhoodRadius = rhs.neighborhoodRadius;
    neighborhoodAngle = rhs.neighborhoodAngle;
    maxForce_ = rhs.maxForce();
    maxSpeed_ = rhs.maxSpeed();
    //boid_color.r = rhs.boid_color.r;
    //boid_color.g = rhs.boid_color.g;
    // boid_color.b = rhs.boid_color.b;
    trail_length = rhs.trail_length;

    return *this;
}

bool Boid::operator==(const Boid &other) const {
    if ( (separationWeight == other.separationWeight) &&
         (alignmentWeight == other.alignmentWeight) &&
         (cohesionWeight == other.cohesionWeight) &&
         (separationRadius == other.separationRadius) &&
         (neighborhoodRadius == other.neighborhoodRadius) &&
         (neighborhoodAngle == other.neighborhoodAngle) &&
         (maxForce_ == other.maxForce()) &&
         (maxSpeed_ == other.maxSpeed()) &&
         (boid_color.r == other.boid_color.r) &&
         (boid_color.g == other.boid_color.g) &&
         (boid_color.b == other.boid_color.b) &&
         (trail_length == other.trail_length) )
        return true;
    else
        return false;
}

bool Boid::operator!=(const Boid &other) const {
    return !(*this == other);
}
